﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO.Ports;
// got from http://www.codeproject.com/KB/cs/nxtBluetooth.aspx -brandon
namespace SwarmMappingAlpha.nxtComm
{
    public class NXTComm
    {

        //Class to read and write over serial com.
        private SerialPort _BluetoothConnection;

        // Declare a 2 bytes vector to store the message length header 
        private Byte[] MessageLength= {0x00, 0x00};

        //Delegate and events used for updating those who care about the drones from RR
        public delegate void message(int[] recievedMessage, int instructionType);
        public event message messageReceived;

        public delegate void updateStatusBox(string message);
        public event updateStatusBox rxtx;

        /// <summary>
        /// Creates a new NXTComm object
        /// </summary>
        public NXTComm()
        {
            _BluetoothConnection = new SerialPort();
            _BluetoothConnection.PortName = "COM11";
            _BluetoothConnection.ReadTimeout = 5000;
            _BluetoothConnection.ReceivedBytesThreshold = 8;

            //When we recieve data parse it
            _BluetoothConnection.DataReceived += new SerialDataReceivedEventHandler(_BluetoothConnection_DataReceived);
        }

        /// <summary>
        /// Parses the data pulled from the buffer, sends it off to get cleaned up
        /// then fires an event with the payload to any listeners
        /// </summary>
        /// <param name="sender">the comm port object</param>
        /// <param name="e">the event</param>
        private void _BluetoothConnection_DataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            SerialPort btPort = (SerialPort)sender;

            //The first 2 bytes are the overall packet size
            int packetSize = btPort.ReadByte();
            packetSize += btPort.ReadByte() << 8;

            byte[] buffer = new byte[packetSize];

            btPort.Read(buffer,0,packetSize);rxtx(payloadToString(buffer));
            //send the payload off to be parsed for the data we need
            int[] parsedPayload = parsePayload(buffer);

            //If we have an event subscriber deal with the data
            if (messageReceived != null)
            {
                //send the message off to be dealt with
                messageReceived(parsedPayload, buffer[4]);
            }
            
        }


        /// <summary>
        /// Parse the payload data based on the instruction type
        /// If we get bad data return an array with -1
        /// </summary>
        /// <param name="payload">The byte array recieved over the comm port</param>
        /// <returns>The parsed array</returns>
        private int[] parsePayload(byte[] payload)
        {
            int[] resultPayload;

            //Next is the command type, should be 0x80: direct command no response needed
            int commandType = (int) payload[0];

            //Next is the command itself, should be 0x09 Message Write
            int command = (int)payload[1];

            // MailBox Number -Isn't used, could be used for a message queue
            int mailboxNumber = (int)payload[2];

            // Next is payload length
            int payloadLength = (int)payload[3];

            switch (payload[4])
            {
                //We recieved a request for confirmation of location from a drone (Am I where I think I am?) - of course not little drone >:)>
                // [ x, y ]
                case 0x03:

                    //Create a new array to hold the result
                    resultPayload = new int[2];

                    //Calculate the x coordinate
                    resultPayload[0] = (int) payload[5];
                    resultPayload[0] += ((int)payload[6]) << 8;

                    //Calculate the y coordinate
                    resultPayload[1] = (int)payload[7];
                    resultPayload[1] += ((int)payload[8]) << 8;
                    return resultPayload;
                    

                //We recieved an edge!
                // [ x1, y1, x2, y2, weight ]
                case 0x04:

                    //Create a new array to hold the result
                    resultPayload = new int[5];

                    //Calculate the x1 coordinate
                    resultPayload[0] = (int)payload[5];
                    resultPayload[0] += ((int)payload[6]) << 8;

                    //Calculate the y1 coordinate
                    resultPayload[1] = (int)payload[7];
                    resultPayload[1] += ((int)payload[8]) << 8;

                    //Calculate the x2 coordinate
                    resultPayload[2] = (int)payload[9];
                    resultPayload[2] += ((int)payload[10]) << 8;

                    //Calculate the y2 coordinate
                    resultPayload[3] = (int)payload[11];
                    resultPayload[3] += ((int)payload[12]) << 8;

                    //Calculate the Weight
                    resultPayload[4] = (int)payload[13];

                    return resultPayload;

                //If we get bad data return an array with -1
                default:
                    resultPayload = new int[1];
                    resultPayload[0] = -1;
                    return resultPayload;
            }
        }

        
        
        /// <summary>
        /// Close the comm port
        /// </summary>
        public void closeConnection()
        {
            if (_BluetoothConnection.IsOpen)
                _BluetoothConnection.Close();
        }

        /// <summary>
        /// open the comm port
        /// </summary>
        public void openConnection()
        {
            if (!_BluetoothConnection.IsOpen)
                _BluetoothConnection.Open();
        }

        /// <summary>
        /// Send data over the comPort
        /// </summary>
        /// <param name="message">The byte array to send over the comm port</param>
        private void writeData(byte[] message)
        {
            openConnection();
            //set the LSB to the length of the message 
            MessageLength[0] = (byte) message.Length;
            MessageLength[1] = (byte)(message.Length >> 8);
            //send the 2 bytes header 
            _BluetoothConnection.Write(MessageLength, 0, MessageLength.Length);
            
            //send the actual payload
            _BluetoothConnection.Write(message, 0, message.Length);
            rxtx("Tx: " + payloadToString(message));
        }

        public void writeCommunication(int heading, int distance)
        {
            //payload command type, write to mailbox, mailbox # -1, payload length, inst type, compass heading lsb, compass heading gsb, distance lsb, distance gsb, termination buffer 
            byte[] payload = { 0x80, 0x09, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
            
            payload[5] = (byte)heading;
            payload[6] = (byte)(heading >> 8);
            payload[7] = (byte)distance;
            payload[8] = (byte)(distance >> 8);

            writeData(payload);
           
        }

        public void writeCommunication(int heading, int distance, int x1, int y1, int x2, int y2)
        {
            //payload command type, write to mailbox, mailbox # -1, payload length, inst type, compass heading lsb, compass heading gsb, distance lsb, distance gsb, termination buffer 
            byte[] payload = { 0x80, 0x09, 0x00, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };

            payload[4] = (byte)1;
            payload[5] = (byte)heading;
            payload[6] = (byte)(heading >> 8);
            payload[7] = (byte)distance;
            payload[8] = (byte)(distance >> 8);
            payload[9] = (byte)x1;
            payload[10] = (byte)(x1 >> 8);
            payload[11] = (byte)y1;
            payload[12] = (byte)(y1 >> 8);
            payload[13] = (byte)x2;
            payload[14] = (byte)(x2 >> 8);
            payload[15] = (byte)y2;
            payload[16] = (byte)(y2 >> 8);


            writeData(payload);


        }


        /// <summary>
        /// Send A True Packet or a Success Packet, Drones use this as recieving true to condition
        /// </summary>
        public void writeCommunication()
        {
            byte[] payload = { 0x80, 0x09, 0x00, 0x02, 0x02, 0x00 };
            writeData(payload);
            rxtx("Tx: " + payloadToString(payload));
        }

        private string payloadToString(byte[] payload)
        {
            string payloadAsString = "";
            foreach (byte payloadData in payload)
            {
                payloadAsString += payloadData.ToString() + ",";
            }

            return payloadAsString;
        }
    }
}
